#include "rclcpp/rclcpp.hpp"
#include "robot_control/srv/robot_state_srv.hpp"
#include "std_srvs/srv/empty.hpp"

#include <chrono>
#include <memory>

using namespace std::chrono_literals;

class ControlClient : public rclcpp::Node
{
public:
    ControlClient() : Node("control_client")
    {
        // 创建服务客户端
        state_client_ = this->create_client<robot_control::srv::RobotStateSrv>("/get_robot_state");
        reset_client_ = this->create_client<std_srvs::srv::Empty>("/reset_robot");
        
        RCLCPP_INFO(this->get_logger(), "控制客户端已启动");
    }
    
    void call_get_state()
    {
        // 等待服务可用
        if (!state_client_->wait_for_service(1s)) {
            RCLCPP_ERROR(this->get_logger(), "状态查询服务不可用");
            return;
        }
        
        auto request = std::make_shared<robot_control::srv::RobotStateSrv::Request>();
        
        // 异步调用服务
        auto future = state_client_->async_send_request(request);
        
        // 等待响应
        if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), future) ==
            rclcpp::FutureReturnCode::SUCCESS)
        {
            auto response = future.get();
            RCLCPP_INFO(this->get_logger(), "机器人状态: %s, 电量: %.1f%%", 
                       response->state.c_str(), response->battery);
        } else {
            RCLCPP_ERROR(this->get_logger(), "服务调用失败");
        }
    }
    
    void call_reset()
    {
        // 等待服务可用
        if (!reset_client_->wait_for_service(1s)) {
            RCLCPP_ERROR(this->get_logger(), "重置服务不可用");
            return;
        }
        
        auto request = std::make_shared<std_srvs::srv::Empty::Request>();
        
        // 异步调用服务
        auto future = reset_client_->async_send_request(request);
        
        // 等待响应
        if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), future) ==
            rclcpp::FutureReturnCode::SUCCESS)
        {
            RCLCPP_INFO(this->get_logger(), "机器人重置成功");
        } else {
            RCLCPP_ERROR(this->get_logger(), "重置服务调用失败");
        }
    }

private:
    rclcpp::Client<robot_control::srv::RobotStateSrv>::SharedPtr state_client_;
    rclcpp::Client<std_srvs::srv::Empty>::SharedPtr reset_client_;
};

int main(int argc, char * argv[])
{
    rclcpp::init(argc, argv);
    
    if (argc < 2) {
        RCLCPP_ERROR(rclcpp::get_logger("control_client"), "用法: control_client <command>");
        RCLCPP_ERROR(rclcpp::get_logger("control_client"), "命令: get_state 或 reset");
        return 1;
    }
    
    std::string command = argv[1];
    auto client = std::make_shared<ControlClient>();
    
    if (command == "get_state") {
        client->call_get_state();
    } else if (command == "reset") {
        client->call_reset();
    } else {
        RCLCPP_ERROR(rclcpp::get_logger("control_client"), "未知命令: %s", command.c_str());
        RCLCPP_ERROR(rclcpp::get_logger("control_client"), "可用命令: get_state, reset");
        return 1;
    }
    
    rclcpp::shutdown();
    return 0;
}